#ifndef _NE_QUATERNION_H_
#define _NE_QUATERNION_H_

#include <mathematics/Matrix3.h>
#include <mathematics/Vector4.h>

namespace ne
{
    class NEAPI Quaternion
    {
        public:
            inline Quaternion()
                : x(0.0f), y(0.0f), z(0.0f), w(1.0f)
            {
            }

            inline Quaternion(const Quaternion &q)
                : x(q.x), y(q.y), z(q.z), w(q.w)
            {
            }

            inline explicit Quaternion(const Vector4 &v)
                : x(v.x), y(v.y), z(v.z), w(v.w)
            {
            }

            inline explicit Quaternion(const Matrix3 &m)
            {
                fromRotationMatrix(m);
            }

            inline explicit Quaternion(const real* const v)
                : x(v[0]), y(v[1]), z(v[2]), w(v[3])
            {
            }

            inline explicit Quaternion(const Vector3* const axes)
            {
                fromAxes(axes);
            }

            inline Quaternion(const real qx, const real qy, const real qz, const real qw)
                : x(qx), y(qy), z(qz), w(qw)
            {
            }

            inline Quaternion(const Vector3 &axis, const real a)
            {
                fromAxisAngle(axis, a);
            }

            inline Quaternion(const Vector3 &vX, const Vector3 &vY, const Vector3 &vZ)
            {
                fromAxes(vX, vY, vZ);
            }

            inline void swap(Quaternion &v)
            {
                std::swap(x, v.x);
                std::swap(y, v.y);
                std::swap(z, v.z);
                std::swap(w, v.w);
            }

            inline real operator [] (const size_t v) const
            {
                assert(v < 4);
                return *(&x + v);
            }

            inline real& operator [] (const size_t v)
            {
                assert(v < 4);
                return *(&x + v);
            }

            inline real* ptr()
            {
                return &x;
            }

            inline const real* ptr() const
            {
                return &x;
            }

            inline Quaternion& operator = (const Quaternion &q)
            {
                x = q.x;
                y = q.y;
                z = q.z;
                w = q.w;
                return *this;
            }

            inline Quaternion& operator = (const Vector4 &v)
            {
                x = v.x;
                y = v.y;
                z = v.z;
                w = v.w;
                return *this;
            }

            inline bool operator == (const Quaternion &q) const
            {
                return (x == q.x) && (y == q.y) && (z == q.z) && (w == q.w);
            }

            inline bool operator != (const Quaternion &q) const
            {
                return (x != q.x) || (y != q.y) || (z != q.z) || (w != q.w);
            }

            inline Quaternion operator + (const Quaternion &q) const
            {
                return Quaternion(x + q.x, y + q.y, z + q.z, w + q.w);
            }

            inline Quaternion operator - (const Quaternion &q) const
            {
                return Quaternion(x - q.x, y - q.y, z - q.z, w - q.w);
            }

            // Q * q != q * Q
            // Q * q is relative Q's axes rotate q's angle
            // q * Q is absolutly axes rotate q's angle
            // It's different with standard crossproduct
            inline Quaternion operator * (const Quaternion &q) const
            {
                return Quaternion(
                    w * q.x + x * q.w + y * q.z - z * q.y,
                    w * q.y + y * q.w + z * q.x - x * q.z,
                    w * q.z + z * q.w + x * q.y - y * q.x,
                    w * q.w - x * q.x - y * q.y - z * q.z);
            }

            inline Quaternion operator - () const
            {
                return Quaternion(-x, -y, -z, -w);
            }

            // TODO: Use Q * v
            // TODO: Do not use v * Q
            inline Vector3 operator * (const Vector3 &v) const
            {
                return rotate(v);
            }

            inline Quaternion operator * (const real fScale) const
            {
                return Quaternion(x * fScale, y * fScale, z * fScale, w * fScale);
            }

            NEAPI friend Quaternion operator * (const real fScale, const Quaternion &q);

            inline void identity()
            {
                x = 0.0f;
                y = 0.0f;
                z = 0.0f;
                w = 1.0f;
            }

            // |Q|
            inline real length() const
            {
                return std::sqrt((x * x) + (y * y) + (z * z) + (w * w));
            }

            inline real squaredLength() const
            {
                return (x * x) + (y * y) + (z * z) + (w * w);
            }

            // The cos of the angle between the two Quaternions
            inline real dotProduct(const Quaternion &q) const
            {
                return (x * q.x) + (y * q.y) + (z * q.z) + (w * q.w);
            }

            // It's different with *
            inline Quaternion crossProduct(const Quaternion &q) const
            {
                return Quaternion(
                    w * q.x + x * q.w + z * q.y - y * q.z,
                    w * q.y + y * q.w + x * q.z - z * q.x,
                    w * q.z + z * q.w + y * q.x - x * q.y,
                    w * q.w - x * q.x - y * q.y - z * q.z);
            }

            // Q*
            inline Quaternion conjugate() const
            {
                return Quaternion(-x, -y, -z, w);
            }

            // q = 1 / Q
            inline bool inverse(Quaternion &q) const
            {
                real len = squaredLength();
                if (len > 0.0f)
                {
                    real invLen = 1.0f / len;
                    q.x = -x * invLen;
                    q.y = -y * invLen;
                    q.z = -z * invLen;
                    q.w =  w * invLen;
                    return true;
                }

                q.identity();
                return false;
            }

            // 1 / Q
            inline Quaternion inverse() const
            {
                Quaternion resQ;
                inverse(resQ);
                return resQ;
            }

            inline real normalize()
            {
                real len = length();
                if (len > 0.0f)
                {
                    real invLen = 1.0f / len;
                    x *= invLen;
                    y *= invLen;
                    z *= invLen;
                    w *= invLen;
                    return len;
                }

                identity();
                return 0.0f;
            }
        
            inline Vector3 rotate(const Vector3 &v) const
            {
                // NVidia SDK implementation
                Vector3 qvec(x, y, z);
                Vector3 uv = qvec.crossProduct(v);
                Vector3 uuv = qvec.crossProduct(uv);
                uv *= (2.0f * w);
                uuv *= 2.0f;
                return v + uv + uuv;
            }

            // R * Q = qDest
            // Get the R
            inline Quaternion getRotation(const Quaternion &qDest) const
            {
                return qDest * inverse();
            }

            // Q * R = qDest
            // Get the R
            inline Quaternion getRotationRelative(const Quaternion &qDest) const
            {
                return inverse() * qDest;
            }

            void fromRotationMatrix(const Matrix3 &m);
            void toRotationMatrix(Matrix3 &m) const;

            void fromAxes(const Vector3* const axes);
            void fromAxes(const Vector3 &vX, const Vector3 &vY, const Vector3 &vZ);
            void toAxes(Vector3 &vX, Vector3 &vY, Vector3 &vZ) const;

            void fromAxisAngle(const Vector3 &axis, const real a);
            bool toAxisAngle(Vector3 &axis, real &a) const;
            real getAngle() const;
            Vector3 getAxis() const;

            void fromEulerAngles(const real yaw, const real pitch, const real roll);
            void toEulerAngles(real &yaw, real &pitch, real &roll) const;

            void fromAxisXAngle(const real a);
            void fromAxisYAngle(const real a);
            void fromAxisZAngle(const real a);

            static Quaternion MakeRotation(const Vector3 &vs, const Vector3 &vd);

            static Quaternion Lerp(const Quaternion &pQ, const Quaternion &qQ,
                                   const real u, const bool shortestPath = false);

            static Quaternion Slerp(const Quaternion &pQ, const Quaternion &qQ,
                                    const real u, const bool shortestPath = false);

            static Quaternion Squad(const Quaternion &pQ, const Quaternion &aQ,
                                    const Quaternion &bQ, const Quaternion &qQ,
                                    const real u, const bool shortestPath = false);

            NEAPI friend std::ostream& operator << (std::ostream &o, const Quaternion &q);

            inline ~Quaternion()
            {
            }

        public:
            static const Quaternion ZERO;
            static const Quaternion IDENTITY;

        public:
            real x, y, z, w;
    };
}

#endif
